#include "ljj_spi.hpp"

#define	SENDONLY	0

LJJSPI::LJJSPI(I2CSPIBusOption bus_option, int bus, int32_t device, int bus_frequency,
	       spi_mode_e spi_mode) :
	SPI(DRV_DEVTYPE_LJJ, MODULE_NAME, bus, device, spi_mode, bus_frequency),
	I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
	ModuleParams(nullptr),
	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read"))
{
	// strncpy(dbg_key.key, "velx", 10);
	// dbg_key.value = 0.0f;
	// pub_dbg_key = orb_advertise(ORB_ID(debug_key_value), &dbg_key);
}



LJJSPI::~LJJSPI()
{
	/* delete the perf counter */
	perf_free(_sample_perf);
}

void LJJSPI::parameters_update()
{
	if (_parameter_update_sub.updated()) {
		parameter_update_s param_update;
		_parameter_update_sub.copy(&param_update);

		param_get(param_find("LJJ_SPI_SENDONLY"), &sendonly);

	}
}


int LJJSPI::init()
{
	int ret = PX4_ERROR;
	/* do SPI init (and probe) first */
	if (SPI::init() != OK) {
		goto out;
	}
	parameters_update();
	/* wait 10 ms (datasheet incorrectly lists 10 us on page 49) */
	usleep(10000);

	ret = OK;

	start();

out:
	return ret;
}



int LJJSPI::probe()
{
	if (1) {
		return OK;
	}
	return -EIO;
}



uint16_t  LJJSPI::read_write(uint8_t reg, uint16_t value)
{
	uint8_t cmd[5] {};
	uint16_t rtvalue;
	cmd[0] = reg ;
	cmd[1] = (uint8_t)(value  >> 8);
	cmd[2] = (uint8_t)( value  & 0x00ff);
	cmd[3] = 0xff;
	cmd[4] = 0xff;
	transfer(cmd, cmd, sizeof(cmd));
	rtvalue = (cmd[3] << 8) + cmd[4];
	return rtvalue;
}



void  LJJSPI::writeOnly(uint8_t reg, uint16_t value)
{
	uint8_t cmd[3] {};
	cmd[0] = reg ;
	cmd[1] = (uint8_t)(value  >> 8);
	cmd[2] = (uint8_t)( value  & 0x00ff);
	transfer(cmd, nullptr, sizeof(cmd));
}



void LJJSPI::start()
{
	/* start polling at the specified rate 500Hz*/
	ScheduleOnInterval(2000, 10000);
}



void LJJSPI::RunImpl()
{
	/* start the performance counter */
	perf_begin(_sample_perf);
	uint64_t timestamp_us = hrt_absolute_time();
	static uint16_t recive = 0;
	parameters_update();
	if(sendonly == 0){

		if (_muc_send_sub.updated()) {
			if (_muc_send_sub.copy(&spi_send)) {
				read_write(spi_send.reg, spi_send.value);
			}
		}else{
			recive = read_write(0x55, 0);
			spi_recive.timestamp = timestamp_us;
			spi_recive.value = recive;
			_muc_recive_pub.publish(spi_recive);
		}
	}else{
		if (_muc_send_sub.updated()) {
			if (_muc_send_sub.copy(&spi_send)) {
				writeOnly(spi_send.reg, spi_send.value);
			}
		}
	}



	/* send debug value, see  NAMED_VALUE_FLOAT in MAVLink Inspector*/
	// uint64_t timestamp_us = hrt_absolute_time();
	// static uint16_t recive = 0;
	// recive ++;
	// uint16_t rrecive;
	// rrecive = read_write((uint8_t)(recive >> 8), recive);
	// dbg_key.value = rrecive;
	// dbg_key.timestamp = timestamp_us;
	// orb_publish(ORB_ID(debug_key_value), pub_dbg_key, &dbg_key);

	/* stop the perf counter */
	perf_end(_sample_perf);
}



void LJJSPI::print_status()
{
	I2CSPIDriverBase::print_status();
	perf_print_counter(_sample_perf);
}



void LJJSPI::print_usage()
{
	PRINT_MODULE_USAGE_NAME("LJJSPI", "driver");
	PRINT_MODULE_USAGE_SUBCATEGORY("LJJSPI111");
	PRINT_MODULE_USAGE_COMMAND("start");
	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}



I2CSPIDriverBase *LJJSPI::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
				      int runtime_instance)
{
	LJJSPI *instance = new LJJSPI(iterator.configuredBusOption(), iterator.bus(), iterator.devid(),
				      cli.bus_frequency, cli.spi_mode);

	if (!instance) {
		PX4_ERR("alloc failed");
		return nullptr;
	}

	if (OK != instance->init()) {
		delete instance;
		return nullptr;
	}

	return instance;
}



extern "C" __EXPORT int ljj_spi_main(int argc, char *argv[])
{
	int ch;
	using ThisDriver = LJJSPI;
	BusCLIArguments cli{false, true};
	cli.default_spi_frequency = 4000000;

	while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
		switch (ch) {
		case 'R':
			cli.rotation = (enum Rotation)atoi(cli.optarg());
			break;
		}
	}

	const char *verb = cli.optarg();

	if (!verb) {
		ThisDriver::print_usage();
		return -1;
	}

	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DEVTYPE_LJJ);

	if (!strcmp(verb, "start")) {
		return ThisDriver::module_start(cli, iterator);
	}

	if (!strcmp(verb, "stop")) {
		return ThisDriver::module_stop(iterator);
	}

	if (!strcmp(verb, "status")) {
		return ThisDriver::module_status(iterator);
	}

	ThisDriver::print_usage();
	return -1;
}
